/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    can.c
  * @brief   This file provides code for the configuration
  *          of the CAN instances.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "can.h"

/* USER CODE BEGIN 0 */

#include "headfiles.h"

#define CAN_FrameID_Motor_Rudder    (0x201)     // 舵电机反馈报文
#define CAN_FrameID_Motor_Wheel     (0x202)     // 轮电机反馈报文


extern int get_fix_point;
extern int count1, count2;
extern struct fix_point_s fix_points_flash[128];
extern float factor[20];
extern int enc_zero_tmp;

/* USER CODE END 0 */

CAN_HandleTypeDef hcan1;
CAN_HandleTypeDef hcan2;

/* CAN1 init function */
void MX_CAN1_Init(void)
{

  /* USER CODE BEGIN CAN1_Init 0 */

  /* USER CODE END CAN1_Init 0 */

  /* USER CODE BEGIN CAN1_Init 1 */

  /* USER CODE END CAN1_Init 1 */
  hcan1.Instance = CAN1;
  hcan1.Init.Prescaler = 4;
  hcan1.Init.Mode = CAN_MODE_NORMAL;
  hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
  hcan1.Init.TimeSeg1 = CAN_BS1_5TQ;
  hcan1.Init.TimeSeg2 = CAN_BS2_3TQ;
  hcan1.Init.TimeTriggeredMode = DISABLE;
  hcan1.Init.AutoBusOff = ENABLE;
  hcan1.Init.AutoWakeUp = ENABLE;
  hcan1.Init.AutoRetransmission = ENABLE;
  hcan1.Init.ReceiveFifoLocked = DISABLE;
  hcan1.Init.TransmitFifoPriority = DISABLE;
  if (HAL_CAN_Init(&hcan1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CAN1_Init 2 */

  /* USER CODE END CAN1_Init 2 */

}
/* CAN2 init function */
void MX_CAN2_Init(void)
{

  /* USER CODE BEGIN CAN2_Init 0 */

  /* USER CODE END CAN2_Init 0 */

  /* USER CODE BEGIN CAN2_Init 1 */

  /* USER CODE END CAN2_Init 1 */
  hcan2.Instance = CAN2;
  hcan2.Init.Prescaler = 4;
  hcan2.Init.Mode = CAN_MODE_NORMAL;
  hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ;
  hcan2.Init.TimeSeg1 = CAN_BS1_5TQ;
  hcan2.Init.TimeSeg2 = CAN_BS2_3TQ;
  hcan2.Init.TimeTriggeredMode = DISABLE;
  hcan2.Init.AutoBusOff = ENABLE;
  hcan2.Init.AutoWakeUp = ENABLE;
  hcan2.Init.AutoRetransmission = ENABLE;
  hcan2.Init.ReceiveFifoLocked = DISABLE;
  hcan2.Init.TransmitFifoPriority = DISABLE;
  if (HAL_CAN_Init(&hcan2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CAN2_Init 2 */

  /* USER CODE END CAN2_Init 2 */

}

static uint32_t HAL_RCC_CAN1_CLK_ENABLED=0;

void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(canHandle->Instance==CAN1)
  {
  /* USER CODE BEGIN CAN1_MspInit 0 */

  /* USER CODE END CAN1_MspInit 0 */
    /* CAN1 clock enable */
    HAL_RCC_CAN1_CLK_ENABLED++;
    if(HAL_RCC_CAN1_CLK_ENABLED==1){
      __HAL_RCC_CAN1_CLK_ENABLE();
    }

    __HAL_RCC_GPIOA_CLK_ENABLE();
    /**CAN1 GPIO Configuration
    PA11     ------> CAN1_RX
    PA12     ------> CAN1_TX
    */
    GPIO_InitStruct.Pin = GPIO_PIN_11;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = GPIO_PIN_12;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

    /* CAN1 interrupt Init */
    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
  /* USER CODE BEGIN CAN1_MspInit 1 */

  /* USER CODE END CAN1_MspInit 1 */
  }
  else if(canHandle->Instance==CAN2)
  {
  /* USER CODE BEGIN CAN2_MspInit 0 */

  /* USER CODE END CAN2_MspInit 0 */
    /* CAN2 clock enable */
    __HAL_RCC_CAN2_CLK_ENABLE();
    HAL_RCC_CAN1_CLK_ENABLED++;
    if(HAL_RCC_CAN1_CLK_ENABLED==1){
      __HAL_RCC_CAN1_CLK_ENABLE();
    }

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**CAN2 GPIO Configuration
    PB12     ------> CAN2_RX
    PB13     ------> CAN2_TX
    */
    GPIO_InitStruct.Pin = GPIO_PIN_12;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = GPIO_PIN_13;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    /* CAN2 interrupt Init */
    HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
  /* USER CODE BEGIN CAN2_MspInit 1 */

  /* USER CODE END CAN2_MspInit 1 */
  }
}

void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
{

  if(canHandle->Instance==CAN1)
  {
  /* USER CODE BEGIN CAN1_MspDeInit 0 */

  /* USER CODE END CAN1_MspDeInit 0 */
    /* Peripheral clock disable */
    HAL_RCC_CAN1_CLK_ENABLED--;
    if(HAL_RCC_CAN1_CLK_ENABLED==0){
      __HAL_RCC_CAN1_CLK_DISABLE();
    }

    /**CAN1 GPIO Configuration
    PA11     ------> CAN1_RX
    PA12     ------> CAN1_TX
    */
    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_11|GPIO_PIN_12);

    /* CAN1 interrupt Deinit */
    HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
  /* USER CODE BEGIN CAN1_MspDeInit 1 */

  /* USER CODE END CAN1_MspDeInit 1 */
  }
  else if(canHandle->Instance==CAN2)
  {
  /* USER CODE BEGIN CAN2_MspDeInit 0 */

  /* USER CODE END CAN2_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_CAN2_CLK_DISABLE();
    HAL_RCC_CAN1_CLK_ENABLED--;
    if(HAL_RCC_CAN1_CLK_ENABLED==0){
      __HAL_RCC_CAN1_CLK_DISABLE();
    }

    /**CAN2 GPIO Configuration
    PB12     ------> CAN2_RX
    PB13     ------> CAN2_TX
    */
    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12|GPIO_PIN_13);

    /* CAN2 interrupt Deinit */
    HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
  /* USER CODE BEGIN CAN2_MspDeInit 1 */

  /* USER CODE END CAN2_MspDeInit 1 */
  }
}

/* USER CODE BEGIN 1 */


/**
  * @name   CAN_FilterConfig()
  * @brief  对CAN的滤波进行配置
  * @call   Internal
  * @param  NULL
  * @RetVal NULL
  */
HAL_StatusTypeDef CAN_FilterConfig(void) 
{
    CAN_FilterTypeDef sFilterConfig;

    sFilterConfig.FilterBank    = 0;   //chenal 0
    sFilterConfig.FilterMode    = CAN_FILTERMODE_IDMASK;   //标识符屏蔽位模式
    sFilterConfig.FilterScale   = CAN_FILTERSCALE_32BIT;  //过滤器位宽为单个32位
    sFilterConfig.FilterIdHigh  = 0x0000;  //标识符寄存器   
    sFilterConfig.FilterIdLow   = 0x0000;   //标识符寄存器  
    sFilterConfig.FilterMaskIdHigh  = 0x0000;   //屏蔽寄存器  //只存在于标识符屏蔽位模式中，在标识符列表模式中为标识符寄存器 
    sFilterConfig.FilterMaskIdLow   = 0x0000;    //屏蔽寄存器                                 
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; //FIFO0的中断和FIFO1的中断是不一样的，这里是把接收到的报文放入到FIFO0中
    sFilterConfig.FilterActivation = CAN_FILTER_ENABLE;   //enable filter
    if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
        return HAL_ERROR;
    }
    if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)  //注册CAN_IT_RX_FIFO0_MSG_PENDING 对应的回调函数原型
    {
        Error_Handler();
        return HAL_ERROR;
    }
    HAL_CAN_Start(&hcan1);
    
    sFilterConfig.FilterBank = 14;
    sFilterConfig.SlaveStartFilterBank = 14;    //为从属can选择开始的过滤库
    
    if (HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK)
    {
        Error_Handler();
        return HAL_ERROR;
    }
    if (HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)  //注册CAN_IT_RX_FIFO0_MSG_PENDING 对应的回调函数原型
    {
        Error_Handler();
        return HAL_ERROR;
    }
    HAL_CAN_Start(&hcan2);
    
    
    return HAL_OK;
}/* CAN_FilterConfig() */


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef RxHeader;
    uint8_t RxData[8];
    
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData);
    
    if(CAN1 == hcan->Instance)
    {
        switch(RxHeader.StdId)
        {
            case CAN_FrameID_Motor_Rudder:
            {
                
                break;
            }/* Motor_Rudder */
            case CAN_FrameID_Motor_Wheel:
            {
                break;
            }/* Motor_Wheel */
            default:
            {
                break;
            }/* default */
        }/* switch */
    }/* CAN1 */
    else if(CAN2 == hcan->Instance)
    {
        switch(RxHeader.StdId)
        {
            case CAN_FrameID_Motor_Rudder:
            {
                motor_data.Rudder.lastPosition      = motor_data.Rudder.position;
                
				motor_data.Rudder.position          = (int16_t)RxData[0] << 8 | RxData[1];
				motor_data.Rudder.speed 	        = (int16_t)RxData[2] << 8 | RxData[3];
				motor_data.Rudder.current           = (int16_t)RxData[4] << 8 | RxData[5];
				motor_data.Rudder.temperature       = (int16_t)RxData[6];
                
                motor_data.Rudder.lastPosition      = (motor_data.Rudder.lastPosition == -1) ? motor_data.Rudder.position : motor_data.Rudder.lastPosition;
                
                motor_data.Rudder.turns             = (motor_data.Rudder.lastPosition - motor_data.Rudder.position) >  (8191 / 2) ? motor_data.Rudder.turns + 1 : motor_data.Rudder.turns;
                motor_data.Rudder.turns             = (motor_data.Rudder.lastPosition - motor_data.Rudder.position) < -(8191 / 2) ? motor_data.Rudder.turns - 1 : motor_data.Rudder.turns;
				
                motor_data.Rudder.sumPositionLast   = motor_data.Rudder.sumPosition;
				motor_data.Rudder.sumPosition       = motor_data.Rudder.turns * 8192 + motor_data.Rudder.position;
                
                MA600_Get_Angle(&TestMA600);
                
                factor[5] = ENC2Deg(TestMA600.Angle, 65536, 0);
                factor[6] = ENC2Deg(motor_data.Rudder.position, 8192, enc_zero_tmp);
                factor[7] = factor[6] - factor[5];
                factor[8] = fix(&fix_handle_test, TestMA600.Angle);
                factor[9] = ENC2Deg(factor[8], 65536, 0);
                factor[10] = factor[6] - factor[9];
                
                
                if(get_fix_point == 2)
                {
                    get_fix_point = 1;
                    count1 = fix_handle_test.num_of_fix_point;
                    count2 = 0;
                }
                
                if(count1 >= 0)
                {
                    if(!count2--)
                    {
                        
                        count2 = 1000 / fix_handle_test.num_of_fix_point;
                        
                        // 添加一个新的点
                        fix_points_flash[count1].raw  = TestMA600.Angle;
                        fix_points_flash[count1].real = factor[6] * 65536 / 360.0;
                        
                        count1--;
                        
                    } 
                }
                

                break;
            }/* Motor_Rudder */
            case CAN_FrameID_Motor_Wheel:
            {
                motor_data.Wheel.lastPosition      = motor_data.Wheel.position;
                
				motor_data.Wheel.position          = (int16_t)RxData[0] << 8 | RxData[1];
				motor_data.Wheel.speed 	        = (int16_t)RxData[2] << 8 | RxData[3];
				motor_data.Wheel.current           = (int16_t)RxData[4] << 8 | RxData[5];
				motor_data.Wheel.temperature       = (int16_t)RxData[6];
                
                motor_data.Wheel.lastPosition      = (motor_data.Wheel.lastPosition == -1) ? motor_data.Wheel.position : motor_data.Wheel.lastPosition;
                
                motor_data.Wheel.turns             = (motor_data.Wheel.lastPosition - motor_data.Wheel.position) >  (8191 / 2) ? motor_data.Wheel.turns + 1 : motor_data.Wheel.turns;
                motor_data.Wheel.turns             = (motor_data.Wheel.lastPosition - motor_data.Wheel.position) < -(8191 / 2) ? motor_data.Wheel.turns - 1 : motor_data.Wheel.turns;
				
                motor_data.Wheel.sumPositionLast   = motor_data.Wheel.sumPosition;
				motor_data.Wheel.sumPosition       = motor_data.Wheel.turns * 8192 + motor_data.Wheel.position;
                
                break;
            }/* Motor_Wheel */
            default:
            {
                break;
            }/* default */
        }/* switch */
    }/* CAN2 */
}/* HAL_CAN_RxFifo0MsgPendingCallback() */

/**
  * @name   CAN_TX_CAN2_0x200()
  * @brief  CAN1发送ID为0x200的报文
  * @call   External
  * @param  motor1  电机1的输出值
  * @param  motor2  电机2的输出值
  * @param  motor3  电机2的输出值
  * @param  motor4  电机4的输出值
  * @RetVal NULL
  */
void CAN_TX_CAN2_0x200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    CAN_TxHeaderTypeDef TxHeader;
    uint8_t TxData[8];
    uint32_t MailBox;

    TxHeader.StdId  = 0x200;
    TxHeader.IDE    = CAN_ID_STD;
    TxHeader.RTR    = CAN_RTR_DATA;
    TxHeader.DLC    = 8;
    
    TxData[0] = motor1 >> 8;
    TxData[1] = motor1 & 0xFF;
    TxData[2] = motor2 >> 8;
    TxData[3] = motor2 & 0xff;
    TxData[4] = motor3 >> 8;
    TxData[5] = motor3 & 0xFF;
    TxData[6] = motor4 >> 8;
    TxData[7] = motor4 & 0xFF;

    HAL_CAN_AddTxMessage(&hcan2, &TxHeader, TxData, &MailBox);
}/* CAN_TX_CAN2_0x200() */

/* USER CODE END 1 */

// 更新 CAN 配置，使 CAN1 用于发送数据到舵向和轮电机

void CAN_TX_CAN1_0x200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    CAN_TxHeaderTypeDef TxHeader;
    uint8_t TxData[8];
    uint32_t MailBox;

    TxHeader.StdId  = 0x200;
    TxHeader.IDE    = CAN_ID_STD;
    TxHeader.RTR    = CAN_RTR_DATA;
    TxHeader.DLC    = 8;
    
    TxData[0] = motor1 >> 8;
    TxData[1] = motor1 & 0xFF;
    TxData[2] = motor2 >> 8;
    TxData[3] = motor2 & 0xff;
    TxData[4] = motor3 >> 8;
    TxData[5] = motor3 & 0xFF;
    TxData[6] = motor4 >> 8;
    TxData[7] = motor4 & 0xFF;

    // 确保使用 hcan1 发送数据
    HAL_CAN_AddTxMessage(&hcan1, &TxHeader, TxData, &MailBox);
}

/* USER CODE END 1 */